/**
 * @file nd03b_calib.c
 * @author tongsheng.tang
 * @brief ND03B Calibration functions
 * @version 1.x.x
 * @date 2021-11
 * 
 * @copyright Copyright (c) 2021, Shenzhen Nephotonics Inc.
 * 
 */

#include "nd03b_comm.h"
#include "nd03b_dev.h"
#include "nd03b_data.h"
#include "nd03b_calib.h"
#include "device.h"


/**
 * @brief ND03B Read Error Status 读取错误寄存器状态
 * @param pNxDevice: ND03B模组设备信息结构体指针
 * @return int32_t: 如果通信正常，则返回错误寄存器状态值
 */
int32_t ND03B_ReadErrorStatus(ND03B_Dev_t *pNxDevice){
    int32_t     ret = ND03B_ERROR_NONE;
    int32_t     error;

    ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_ERROR_FLAG, (uint32_t*)&error);
    if(ND03B_ERROR_NONE == ret)
    {
        ret = error;
        NX_PRINTF("Error: %d\r\n", error);
    }

    return ret;
}


/**
 * @brief ND03B Offset Calibration Request 发送偏移量标定请求
 * @param pNxDevice: ND03B模组设备信息结构体指针
 * @return int32_t
 */
int32_t ND03B_OffsetCalibrationRequest(ND03B_Dev_t *pNxDevice)
{
    int32_t     ret = ND03B_ERROR_NONE;

    ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, ND03B_OFFSET_CALIBRATION_REQ_MASK);

    return ret;
}

/**
 * @brief ND03B XTalkCalibration Request 发送串扰标定请求
 * @param pNxDevice: ND03B模组设备信息结构体指针
 * @return int32_t
 */
int32_t ND03B_XTalkCalibrationRequest(ND03B_Dev_t *pNxDevice)
{
    int32_t     ret = ND03B_ERROR_NONE;

    ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, ND03B_XTALK_CALIB_REQ_MASK);

    return ret;
}


/**
 * @brief ND03B Wait for Calibration 等待标定的完成
 * @param pNxDevice: ND03B模组设备信息结构体指针
 * @return int32_t
 */
int32_t ND03B_WaitforOffsetCalibration(ND03B_Dev_t *pNxDevice)
{
	int32_t     ret = ND03B_ERROR_NONE;
	uint32_t    rbuf = 0;
    uint16_t    retry_cnt = 50;
	
    while(retry_cnt)
    {
		ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, &rbuf);
        if(ND03B_OFFSET_CALIBRATION_VAL_MASK == rbuf)
            break;
		ND03B_Delay1ms(100);
        retry_cnt--;
        Device_FeedDog();
	}

    if(retry_cnt == 0)
    {
        NX_PRINTF("Calibration Error\r\n");
        return ND03B_ERROR_TIME_OUT;
    }
    /* 清除标志位 */
    ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, 0x00000000);

    return ret;
}


/**
 * @brief ND03B Wait for XTalk Calibration 等待串扰标定的完成
 * @param pNxDevice: ND03B模组设备信息结构体指针
 * @return int32_t
 */
int32_t ND03B_WaitforXTalkCalibration(ND03B_Dev_t *pNxDevice)
{
	int32_t     ret = ND03B_ERROR_NONE;
	uint32_t    rbuf = 0;
    uint16_t    retry_cnt = 50;

    while(retry_cnt)
    {
		ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, &rbuf);
        if(ND03B_XTALK_CALIB_VAL_MASK == rbuf)
            break;
        ND03B_Delay1ms(100);
        retry_cnt--;
        Device_FeedDog();
	}

    if(retry_cnt == 0)
    {
        return ND03B_ERROR_TIME_OUT;
    }
    /* 清除标志位 */
    ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_DATA_VAL_REQ, 0x00000000);

    return ret;
}


/**
 * @brief   ToF Offset 标定
 * 
 * @param   pNxDevice   设备模组
 * @return  int32_t  
 * @retval  0:  成功
 * @retval  !0: Offset标定失败
 */
int32_t ND03B_OffsetCalibration(ND03B_Dev_t *pNxDevice)
{
    int32_t     ret = ND03B_ERROR_NONE;

    NX_PRINTF("\r\n%s Start!\r\n", __func__);

    if(ND03B_WaitDeviceBootUp(pNxDevice) != ND03B_ERROR_NONE)
    {
        ret = ND03B_ERROR_BOOT;
        return ret;
    }
    ret |= ND03B_SetOffsetCalibDistMM(pNxDevice, 150);    /* 设置Offset标定距离(mm) */
    ret |= ND03B_OffsetCalibrationRequest(pNxDevice);
    // ND03B_Delay1ms(1500);
    ret |= ND03B_WaitforOffsetCalibration(pNxDevice);
    if(ND03B_ERROR_NONE == ret)
        ret |= ND03B_ReadErrorStatus(pNxDevice);

    ND03B_Sleep(pNxDevice);

    NX_PRINTF("%s End!\r\n", __func__);

    return ret;
}

/**
 * @brief    ToF Offset 标定
 * @details  可以指定标定距离
 * 
 * @param   pNxDevice       设备模组
 * @param   calib_depth_mm  标定距离
 * @return  int32_t  
 * @retval  0:  成功
 * @retval  !0: Offset标定失败
 */
int32_t ND03B_OffsetCalibrationAtDepth(ND03B_Dev_t *pNxDevice, uint16_t calib_depth_mm)
{
    int32_t     ret = ND03B_ERROR_NONE;

    NX_PRINTF("\r\n%s Start!\r\n", __func__);

    if(ND03B_WaitDeviceBootUp(pNxDevice) != ND03B_ERROR_NONE)
    {
        ret = ND03B_ERROR_BOOT;
        NX_PRINTF("ret:%d\r\n", ret);
        return ret;
    }
    if(calib_depth_mm != 0)
        ret |= ND03B_SetOffsetCalibDistMM(pNxDevice, calib_depth_mm);    /* 设置Offset标定距离(mm) */
    ret |= ND03B_OffsetCalibrationRequest(pNxDevice);
    ND03B_Delay1ms(1500);
    ret |= ND03B_WaitforOffsetCalibration(pNxDevice);
    if(ND03B_ERROR_NONE == ret)
        ret |= ND03B_ReadErrorStatus(pNxDevice);

    ND03B_Sleep(pNxDevice);

    NX_PRINTF("%s End!\r\n", __func__);

    return ret;
}


/**
 * @brief 串扰标定功能
 * 
 * @note  矫正玻璃盖板串扰
 * 
 * @param   pNxDevice   模组设备       
 * @return  int32_t   
 * @retval  0:  成功
 * @retval  !0: 串扰标定失败
 */
int32_t ND03B_XTalkCalibration(ND03B_Dev_t *pNxDevice)
{
	int32_t     ret = ND03B_ERROR_NONE;
	
    NX_PRINTF("\r\n%s Start!\r\n", __func__);

    if(ND03B_WaitDeviceBootUp(pNxDevice) != ND03B_ERROR_NONE)
    {
        ret = ND03B_ERROR_BOOT;
        return ret;
    }
    ret |= ND03B_SetXTalkCalibDistMM(pNxDevice, 700);      /*! 设置串扰标定距离(mm) */
    ret |= ND03B_XTalkCalibrationRequest(pNxDevice);
    // ND03B_Delay1ms(1500);
    ret |= ND03B_WaitforXTalkCalibration(pNxDevice);
    if(ND03B_ERROR_NONE == ret)
        ret |= ND03B_ReadErrorStatus(pNxDevice);
    
    ND03B_Sleep(pNxDevice);

    NX_PRINTF("%s End!\r\n", __func__);
	
	return ret;
}


/**
 * @brief    串扰标定功能
 * @details  可以指定标定距离
 * 
 * @note  矫正玻璃盖板串扰
 * 
 * @param   pNxDevice               模组设备  
 * @param   xtalk_calib_depth_mm    标定距离    
 * @return  int32_t   
 * @retval  0:  成功
 * @retval  !0: 串扰标定失败
 */
int32_t ND03B_XTalkCalibrationAtDepth(ND03B_Dev_t *pNxDevice, uint16_t xtalk_calib_depth_mm)
{
	int32_t     ret = ND03B_ERROR_NONE;
	
    NX_PRINTF("\r\n%s Start!\r\n", __func__);

    if(ND03B_WaitDeviceBootUp(pNxDevice) != ND03B_ERROR_NONE)
    {
        ret = ND03B_ERROR_BOOT;
        return ret;
    }
    if(xtalk_calib_depth_mm != 0)
        ret |= ND03B_SetXTalkCalibDistMM(pNxDevice, xtalk_calib_depth_mm);      /*! 设置串扰标定距离(mm) */
    ret |= ND03B_XTalkCalibrationRequest(pNxDevice);
    ND03B_Delay1ms(1500);
    ret |= ND03B_WaitforXTalkCalibration(pNxDevice);
    if(ND03B_ERROR_NONE == ret)
        ret |= ND03B_ReadErrorStatus(pNxDevice);

    ND03B_Sleep(pNxDevice);

    NX_PRINTF("%s End!\r\n", __func__);
	
	return ret;
}

/**
 * @brief ND03B Get XTalk Amp
 *        获取串扰幅度值
 * 
 * @note  用于判断串扰是否过大，从而优化盖板
 * 
 * @param   pNxDevice   模组设备   
 * @param   xtalk_amp   串扰幅度值指针
 * @return  int32_t   
 * @retval  0:  运行成功
 * @retval  !0: 运行失败
 */
int32_t ND03B_GetXTalkAmp(ND03B_Dev_t *pNxDevice, int32_t *xtalk_amp)
{
    int32_t     ret = ND03B_ERROR_NONE;
    uint32_t    xtalk_offset_depth;

    switch(pNxDevice->chip_info.nd03b_fw_version){
        case ND03B_FW_VERSION_V123_VALUE:
            ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_V123_XTALK_AMP, (uint32_t*)xtalk_amp);
            *xtalk_amp = *xtalk_amp / 64;
            ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_V123_XTALK_DEPTH , &xtalk_offset_depth);
            if(xtalk_offset_depth == ND03B_DEPTH_LOW_AMP || xtalk_offset_depth == ND03B_DEPTH_OVERFLOW)
            {
                ret = ND03B_ERROR_XTALK_CALIBRATION;
                *xtalk_amp = ND03B_DEPTH_OVERFLOW;
            }
            break;
        default:
            ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_XTALK_AMP, (uint32_t*)xtalk_amp);
            if(ND03B_ERROR_NONE == ret)
                ret |= ND03B_ReadErrorStatus(pNxDevice);
            break;
    }

    return ret;
}


/**
 * @brief ND03B Set Offset Calibration Depth MM
 *        设置偏移量标定深度
 * @param   pNxDevice 模组设备
 * @param   depth_mm   偏移量标定深度 / mm
 * @return  int32_t   
 * @retval  0:  运行成功
 * @retval  !0: 运行失败
 */
int32_t ND03B_SetOffsetCalibDistMM(ND03B_Dev_t *pNxDevice, uint16_t depth_mm)
{
	int32_t     ret = ND03B_ERROR_NONE;
    uint32_t    rbuf;

    ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_CALIB_XT_OFFSET, &rbuf);
    rbuf = (rbuf & 0xFFFF0000) + ((uint32_t)depth_mm & 0x0000FFFF);
	ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_CALIB_XT_OFFSET, rbuf);

	return ret;
}


/**
 * @brief ND03B Set XTalk Calibration Depth MM
 *        设置串扰标定的深度
 * 
 * @note  建议取值范围在60~120cm，推荐使用80cm
 * 
 * @param   pNxDevice   模组设备
 * @param   depth_mm   串扰标定深度 / mm
 * @return  int32_t
 */
int32_t ND03B_SetXTalkCalibDistMM(ND03B_Dev_t *pNxDevice, uint16_t depth_mm)
{
    int32_t     ret = ND03B_ERROR_NONE;
    uint32_t    rbuf;

    ret |= ND03B_ReadWord(pNxDevice, ND03B_REG_CALIB_XT_OFFSET, &rbuf);
    rbuf = (rbuf & 0x0000FFFF) + (((uint32_t)depth_mm<<16) & 0xFFFF0000);
	ret |= ND03B_WriteWord(pNxDevice, ND03B_REG_CALIB_XT_OFFSET, rbuf);

	return ret;
}

